/*
 * apAEPD.cpp
 *
 *  Created on: May 13, 2011
 *      Author: skyang
 */

#include "apAEPD.h"
#include <math.h>
#include <vector>
#include <cmath>
int EDGE = 5;
using namespace std;

/*double abs(double x){
	if (x>0) return x;
	else return -1*x;
}*/



void localAEPD_Fit(double * x, int N, double * params){

	// Take the array in x[0] - x[N-1] and put that into the vector data.
	vector<double> data(x, x+N);

	/*for (int i = 0; i < N; i++){
//		cout << "X[" << i << "] = " << x[i]<< endl;
		cout << "Before Sort data[" << i << "] = "<< data[i]<<endl;
	}*/


	/* Sorts the data: can be done in InArray.updateArray */
	double tmp;
	for(int i = 0;i < N-1 ;i++){
		for (int j = i + 1;j < N;j++){
			if (data[i] > data[j]){
				tmp  = data[i];
				data[i] = data[j];
				data[j] = tmp;
			}
		}
	}


	///sort(data.begin(),data.end());

	//for (int i = 0;i< N; i++){
	//	cout << "data sorted element " << i << " = " << data[i] << endl;
	//}

	//for (int i = 0;i< 4; i++){
	//	cout << "params " << i << " = " << params[i] << endl;
	//}
	double INCREMENT_ALPHA = 0.1;
	int sizeALPHA = 80;
	int NUM_STEPS = 500;

	double inc_A = INCREMENT_ALPHA;//////Iteration Increment of Alpha Parameter
	double start_A = 0.7;//////// Value to Start Alpha Iteration
	int sizeA = sizeALPHA;//// Number of iterations to perform

	double ** Kappa = new double*[sizeA];
	double ** Sigma = new double*[sizeA];
	double ** H = new double*[sizeA];
	for (int i = 0 ; i< sizeA ; i++){
		Kappa[i] = new double[N];
		Sigma[i] = new double[N];
		H[i] = new double[N];
	}

	double * X_minus = new double[sizeA];
	double * X_plus = new double[sizeA];


	int idx_k = 0;
	int idx_q = 0;
	double min_H = 10000000000;

	//int stepX = round(N/NUM_STEPS);
	int stepX = N/NUM_STEPS;
	int X_right = 0;
	int X_left = 0;
	if (stepX <1)
		stepX=1;

	for(int k=EDGE-1 ; k <= N-EDGE-1; k=k+stepX)
	{

		double p1 = data[k];

		X_right = N- k;
		X_left = k ;

		double * x_minus_tmp = new double[X_left];
		double * x_plus_tmp = new double[X_right];

		/* Remember that data[] is already sorted.
		 * Take k as the current index (p1 = data[k]),
		 * x_minus_tmp[0 -> k] becomes absolute value (data - p1)
		 * x_plus_tmp[0 -> N - k] is just data -p1 (because already sorted)
		 */

		for( int q = 0; q< N; q++){
			if (q < X_left)
				x_minus_tmp[q] = abs(data[q] - p1);
			else
				x_plus_tmp[q - X_left] = data[q] - p1;
		}

		/* start_A = 0.7
		 * q = [0 -> sizeA]
		 * inc_A = incrementAlpha
		 * alpha = 0.7 + q * inc_A
		 * Basically alpha goes from the start value through sizeA increments of inc_A 		 *
		 */


		for ( int q = 0; q < sizeA; q++){
			X_minus[q] = 0;
			X_plus[q] = 0;

			double alpha = start_A + q * inc_A;
			/* Goes through the array, If this is matlab then the code would be:
			 * alpha[sizeA] = 0.7 + sizeA*inc_A
			 * data[N] = data vector coming in
			 * k = index value			 *
			 * x_minus_tmp [k];
			 * x_plus_tmp[N-k];
			 * x_minus_tmp = abs( data[0-k] - data[k] );
			 * x_plus_tmp = data[k-N] - data[k];
			 * X_minus[q] = sum (x_minus_tmp.^alpha[q])/N;
			 * X_plus[q] = sum (x_plus_tmp.^alpha[q])/N;
			 *
			 * So the outer loop pins k, the inner loop pins q.
			 * and for each nested loop we have X_minus[k, q] and X_plus [k,q]
			 *
			 */

			for (int t = 0; t < N; t++){
				if (t < X_left)
					X_minus[q] += pow(x_minus_tmp[t], alpha) / N;
				else
					X_plus[q] += pow(x_plus_tmp[t - X_left], alpha)/ N;
			}
		}


		//Produce potential parameter sets as a function of  Alpha, Sigma, Kappa which will be used for maximization
		for (int q = 0; q < sizeA; q++){

			double alpha = start_A + q * inc_A;
			Kappa[q][k] = pow( X_minus[q] / X_plus[q], 1 / (2*(alpha +1)));

			Sigma[q][k] = pow((alpha*  pow( X_minus[q]*X_plus[q], alpha/ (2*(alpha+1)))\
				* ( pow(X_plus[q], 1/(alpha+1)) + pow(X_minus[q], 1/(alpha+1)))), 1/alpha);

			double sgngam = 0;
			H[q][k] = log(Sigma[q][k]) + locallngamma(1/alpha, &sgngam) \
				+ log(Kappa[q][k] + 1/Kappa[q][k]) + 1/alpha - log(alpha);

			//cout << "Kappa : " << Kappa[q* N + k] << "\t" << "Sigma : " << Sigma[q* N + k] << "\t"<< "H :" << H[q* N + k] << endl;
			// keep record of the minimum entropy index
			if(q < sizeA -2 && q >=1 && H[q][k] < min_H ){
				min_H = H[q][k];
				idx_k = k;
				idx_q = q;
			}
		}

		delete [] x_minus_tmp;
		delete [] x_plus_tmp;
	}

	params[0] = data[idx_k]; // theta
	params[1] = Sigma[idx_q][idx_k]; // sigma
	params[2] = Kappa[idx_q][idx_k]; // kappa
	params[3] = start_A + idx_q * inc_A; // alpha

	for (int i = 0 ; i< sizeA ; i++){
		delete [] Kappa[i];
		delete [] Sigma[i];
		delete [] H[i];

	}

	delete [] Kappa;
	delete [] Sigma;
	delete [] H;
	delete [] X_minus;
	delete [] X_plus;
	//cout << "Got here with no problem "<< endl;
}

double locallngamma(double x, double* sgngam)
{

    double a;
    double b;
    double c;
    double p;
    double q;
    double u;
    double w;
    double z;
    int i;
    double logpi;
    double ls2pi;
    double tmp;
    double result;

    *sgngam = 0;

    *sgngam = 1;
    logpi = 1.14472988584940017414;
    ls2pi = 0.91893853320467274178;
    if( x<-34.0 )
    {
        q = -x;
        w = locallngamma(q, &tmp);
        p = floor(q);
        i = floor(p+0.5);
        if( i%2==0 )
        {
            *sgngam = -1;
        }
        else
        {
            *sgngam = 1;
        }
        z = q-p;
        if( z>0.5 )
        {
            p = p+1;
            z = p-q;
        }
        z = q*sin(ae_pi*z);
        result = logpi-log(z)-w;
        return result;
    }
    if( x<13 )
    {
        z = 1;
        p = 0;
        u = x;
        while(u>=3)
        {
            p = p-1;
            u = x+p;
            z = z*u;
        }
        while(u<2)
        {
            z = z/u;
            p = p+1;
            u = x+p;
        }
        if( z<0 )
        {
            *sgngam = -1;
            z = -z;
        }
        else
        {
            *sgngam = 1;
        }
        if( u==2)
        {
            result = log(z);
            return result;
        }
        p = p-2;
        x = x+p;
        b = -1378.25152569120859100;
        b = -38801.6315134637840924+x*b;
        b = -331612.992738871184744+x*b;
        b = -1162370.97492762307383+x*b;
        b = -1721737.00820839662146+x*b;
        b = -853555.664245765465627+x*b;
        c = 1;
        c = -351.815701436523470549+x*c;
        c = -17064.2106651881159223+x*c;
        c = -220528.590553854454839+x*c;
        c = -1139334.44367982507207+x*c;
        c = -2532523.07177582951285+x*c;
        c = -2018891.41433532773231+x*c;
        p = x*b/c;
        result = log(z)+p;
        return result;
    }
    q = (x-0.5)*log(x)-x+ls2pi;
    if(x>100000000 )
    {
        result = q;
        return result;
    }
    p = 1/(x*x);
    if( x>=1000.0 )
    {
        q = q+((7.9365079365079365079365*0.0001*p-2.7777777777777777777778*0.001)*p+0.0833333333333333333333)/x;
    }
    else
    {
        a = 8.11614167470508450300*0.0001;
        a = -5.95061904284301438324*0.0001+p*a;
        a = 7.93650340457716943945*0.0001+p*a;
        a = -2.77777777730099687205*0.001+p*a;
        a = 8.33333333333331927722*0.01+p*a;
        q = q+a/x;
    }
    result = q;
    return result;

}
